#include <csignal>
#include <iostream>
#include <thread>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <atomic>
#include <mutex>
#include <condition_variable>
#include <vector>
#include "globals.h"            // 声明全局变量
#include "z1arm.h"              // Z1ARM 类
#include "delayedParse.h"       // 延时解析队列
#include "unitree_arm_sdk/control/unitreeArm.h"
#include "unitree_arm_sdk/thirdparty/robotics.h"

// ============== 在这里定义globals.h中声明的全局变量 ==============
std::atomic<bool> exit_flag(false);
std::atomic<bool> exit_flag_motor(false);
std::condition_variable force_cv;
std::atomic<bool> g_hasNewPacket(false);
std::mutex g_latestMutex;
std::vector<unsigned char> g_latestPacket;

float force[6] = {0.0f};
std::mutex force_mutex;
std::mutex posture_mutex;
UNITREE_ARM::Vec6 g_serverDesPosture = UNITREE_ARM::Vec6::Zero();
std::atomic<MotorCommand> g_motorCmd(MotorCommand::NONE);
// ================================================================

// ============= 这里声明/实现你的一些函数(或放其他cpp中) =============

// 读串口并解析力数据
void readSerialData(int fd, std::atomic<bool>& exit_flag);
// 发送力数据到服务器
void sendForceThreadFunc(Z1ARM& arm);
// 监控键盘输入
void monitorKeyboard(std::atomic<bool>& exit_flag);

// 安全关节管理
bool safeArmShutdown(Z1ARM& arm);

// ============= 以上函数可在别的 .cpp 中定义，这里省略 =============

// ============ TCP 相关函数(或在别处 .cpp) ==============
void tcpClient(Z1ARM& arm);
void receiveDataFromServer(Z1ARM& arm);

// ============= 一些控制线程(如导纳控制) =============
#ifdef USE_ADMITTANCE_CONTROL
void admittanceControlThreadFunc(Z1ARM& arm);
#endif

// ============= Logging =============
void logArmPosition(Z1ARM& arm, std::atomic<bool>& exit_flag, const std::string& filename);


int main()
{
    // 1) 安装 SIGINT 信号处理
    try {
        std::signal(SIGINT, [](int) {
            std::cout << "\n[main] SIGINT received, initiating graceful shutdown...\n";
            exit_flag = true;
            exit_flag_motor = true;
            force_cv.notify_all();
        });
    } catch (const std::exception& e) {
        std::cerr << "[main] Failed to set up signal handler: " << e.what() << "\n";
    }

    // 2) 初始化输出格式
    std::cout << std::fixed << std::setprecision(3);

    // 3) 初始化串口(力传感器), 例如 ...
    int serial_fd = -1; // 如果成功则 >= 0
    // TODO: 如果有 initSerialPort("/dev/ttyUSB0") 之类的调用

    // 4) 初始化机械臂
    Z1ARM arm;
    if (arm.sendRecvThread != nullptr) {
        arm.sendRecvThread->start();
        std::cout << "[main] ARM communication thread started\n";
    } else {
        std::cerr << "[main] ERROR: ARM communication thread unavailable\n";
        return 1;
    }

    try {
        arm.backToStart();
        arm.setFsm(ArmFSMState::PASSIVE);
        arm.setFsm(ArmFSMState::LOWCMD);

        std::vector<double> KP = arm._ctrlComp->lowcmd->kp;
        std::vector<double> KD = arm._ctrlComp->lowcmd->kd;
        arm._ctrlComp->lowcmd->setControlGain(KP, KD);

        arm.setFsm(ArmFSMState::PASSIVE);
        std::cout << "[main] Arm initialization complete\n";
    }
    catch(const std::exception &e) {
        std::cerr << "[main] Exception init arm: " << e.what() << "\n";
        return 1;
    }

    // 5) 启动TCP客户端线程
    std::thread tcp_thread(tcpClient, std::ref(arm));
    // 6) 启动键盘监控线程
    std::thread kb_thread(monitorKeyboard, std::ref(exit_flag));
    // 7) 启动电机串口线程
    std::thread motor_thread(/* motorSerialThreadFunc */);
    // ...
    tcp_thread.join();
    // 判断是否要退出
    if (exit_flag) {
        safeArmShutdown(arm);
        if(kb_thread.joinable()) kb_thread.join();
        if(motor_thread.joinable()) motor_thread.join();
        // 关arm
        if(arm.sendRecvThread) {
            arm.sendRecvThread->shutdown();
        }
        return 0;
    }

    // 8) 日志文件
    auto now_t = std::time(nullptr);
    char buf[128];
    std::strftime(buf, sizeof(buf), "arm_position_log_%Y%m%d_%H%M%S.txt", std::localtime(&now_t));
    std::string filename(buf);
    std::cout << "[main] Log file=" << filename << "\n";
    std::thread log_thread(logArmPosition, std::ref(arm), std::ref(exit_flag), filename);

    // 9) 启动“延时解析”线程
    std::thread delayedParseThread(delayedParseThreadFunc);

    // 10) 启动接收线程
    std::thread recv_thread(receiveDataFromServer, std::ref(arm));
    // 11) 启动 parseServerThreadFunc
    std::thread parse_thread(/* parseServerThreadFunc */);

    // 12) 如果 serial_fd>=0，则启动 readSerialData
    std::thread force_read_thread;
    if(serial_fd>=0){
        force_read_thread = std::thread(readSerialData, serial_fd, std::ref(exit_flag));
    }

    // 13) 启动 sendForceThreadFunc
    std::thread force_send_thread(/*sendForceThreadFunc, std::ref(arm)*/);

    // 14) 启动控制线程(Admittance / Position / Speed)
#ifdef USE_ADMITTANCE_CONTROL
    std::thread ctrl_thread(admittanceControlThreadFunc, std::ref(arm));
#else
    std::thread ctrl_thread(/* armControlThreadFunc or speed... */);
#endif

    // 15) 主线程：等待退出标志, 进行健康检查
    while(!exit_flag) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        // ...
    }

    // 16) 安全退出: join
    if(recv_thread.joinable()) recv_thread.join();
    if(parse_thread.joinable()) parse_thread.join();
    if(ctrl_thread.joinable()) ctrl_thread.join();
    // ...
    // join 其他
    if(kb_thread.joinable()) kb_thread.join();
    if(motor_thread.joinable()) motor_thread.join();
    if(log_thread.joinable())   log_thread.join();
    if(force_read_thread.joinable()) force_read_thread.join();
    if(force_send_thread.joinable()) force_send_thread.join();
    if(delayedParseThread.joinable()) delayedParseThread.join();

    // 17) shutdown arm
    safeArmShutdown(arm);
    if(arm.sendRecvThread) {
        arm.sendRecvThread->shutdown();
    }
    std::cout << "[main] Clean exit.\n";
    return 0;
}
